#pragma once

#include <algorithm/math.h>
#include <algorithm/matrix.h>

namespace os {
using namespace os::math;

class QuaternionExtendedKalmanFilter {
public:
    Matrix<6, 1> xHat{}; // x(k|k)
    Matrix<6, 1> xHat_{}; // x(k|k-1)

private:
    Matrix<3, 1> z{}; // 测量输入矩阵
    Matrix<6, 6> P{}; // 估计误差协方差矩阵
    Matrix<6, 6> P_{}; // 估计误差协方差矩阵预测
    Matrix<6, 6> F{}; // 状态转移矩阵
    Matrix<3, 6> H{}; // 观测矩阵
    Matrix<6, 6> Q{}; // 过程噪声协方差矩阵
    Matrix<3, 3> R{}; // 观测噪声协方差矩阵
    Matrix<6, 3> K{}; // 卡尔曼增益

private:
    bool _normal_p_update{true};
    bool _converge_flag{false};
    uint8_t _stable_flag{0};
    uint64_t _error_count{0};
    uint64_t _update_count{0};

    float _gyro[3]{0};
    float _accel[3]{0};

    float _orientation_cosine[3]{0};

    float _acc_low_pass_filter_coef{0};
    float _gyro_norm{0};
    float _accl_norm{0};
    float _adaptive_gain_scale{0};

    // 噪声方差
    float _q1{0};
    float _q2{0};
    float _r{0};

    // 卡方校验
    Matrix<1, 1> _chi{0};
    float _chi_threshold{0};
    float _lambda{0};

public:
    QuaternionExtendedKalmanFilter(float process_noise1, float process_noise2, float measure_noise, float lambda,
                                   float lpf);

    QuaternionExtendedKalmanFilter* update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
};
} // namespace os

namespace os {
namespace math {
using Quaternion = struct {
    float32_t w;
    float32_t x;
    float32_t y;
    float32_t z;
};
using EulerAngles = struct {
    radian_t roll;
    radian_t pitch;
    radian_t yaw;
};
} // namespace math
} // namespace os

namespace os {
namespace math {
Quaternion EulerToQuaternions(EulerAngles euler);
EulerAngles QuaternionsToEuler(Quaternion q);
void CorrectEulerAngles(EulerAngles* res, EulerAngles const& src);
} // namespace math
}